/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <vector>
#include "TOccupancyGrid.h"
#include "update_pose.h"
#include "JPEGEncoder.hpp"  ///< For saving JPEG


#define matchfile "matchdataout.txt"
#define outfile "match2dataout.txt"
#define poseset "map_matched_set1.txt"
//"rocla_match_1.txt"

//"map_matched_set1.txt"


int forwardString(const char *buf, int begin,int items){
		int i=begin+1;
		int cnt = 0;
		while(cnt<items){
				if(buf[i]==' ') cnt++;
				i++;
		}
		return i;
}

/// For reading the data from file
int readScanLine(FILE *f,Grid::TScan &meas, Grid::pose &p ){
		float time_s;
		int i=0;
		int maxstring=16096;
		char in_line[maxstring];
		char *ret;
		char c=0;
		
		for(int j=0;j<maxstring;j++) in_line[j]='\n';
		
		ret = fgets (in_line, maxstring-1, f );
		if(ret==NULL){
				if(feof(f)){
						fprintf(stderr,"Reached end of file -- Alles klar!\n");
						return -1;
				}else{
						fprintf(stderr,"Some error occurred...sorry contact Jari :)\n");
						return -1;
				}
		}
		int cnt=0;
		while( (c!='\n') && i<16095){
				if(in_line[i]==' ') cnt++;
				else if(in_line[i]=='\n') break;
				i++;
		}
		if(i==16095) {
			fprintf(stderr,"ERROR reading file!!");
			return -1;
		}
		
		
		//fprintf(stderr,"There is %d floats in this file (= %d meas points)\n",cnt,(cnt-3)/2);
		sscanf(in_line,"%f %f %f ",&p.x,&p.y,&p.a); ///< Pose header
		int pointer_ind=0;
		pointer_ind = forwardString(in_line,0,3); ///search the next begin for float
		meas.N = (cnt-3)/2;
		i =0;
		while(i<((cnt-3)/2)){
				sscanf((in_line+pointer_ind),"%f %f",&meas.r[i],&meas.a[i]);
				pointer_ind = forwardString(in_line,pointer_ind,2); ///search the next begin for float
				i++;
		}
	
		return 0;
}


void writeScanLine(FILE *f,Grid::TScan &meas, float x,float y,float a){
	fprintf(f,"%f %f %f ",x,y,a); ///< Pose
	int i;
	for(i=0;i<meas.N;i++){
		fprintf(f,"%f %f ",meas.r[i],meas.a[i]);
	}
	fprintf(f,"%f %f\n",meas.r[i],meas.a[i]);
}

//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
///DRAWING STUFF
//////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////
void setRGBpix(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b ){
	rgb[3*Nx*j+3*i]=r;
	rgb[3*Nx*j+3*i+1]=g;
	rgb[3*Nx*j+3*i+2]=b;
}

void setRGBmap(unsigned char *rgb,TOccupancyGrid *gr, unsigned char r,unsigned char g, unsigned char b){
	float *data = gr->getDataPtr();
	int Nx,Ny;
	gr->getDim(Nx,Ny);
	float v;
	
	for(int j=0;j<Ny;j++){
		for(int i=0;i<Nx;i++){
			v = data[Nx*j+i];
			if(v>0.6) v = 0; else v = 0.99; 
			
			setRGBpix(rgb,i,j, Nx, r*v,g*v,b*v);
		}
	}	
}

void setRGBpixBold(unsigned char *rgb,int i,int j, int Nx, unsigned char r,unsigned char g, unsigned char b){
	for(int ii=i-1;ii<i+1;ii++){
		for(int jj=j-1;jj<j+1;jj++){
			setRGBpix(rgb,ii,jj,Nx,r,g,b);
		}
	}
	
}

void drawLine(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
							unsigned char r,unsigned char g,unsigned char b){
	float dy = sin(angle);
	float dx = cos(angle);
	float rr=0;
	int i=0;
	uint x,y;
	while(rr<length){
		x = pix + i*dx;
		y = piy - i*dy;
		setRGBpixBold(RGB,x,y, Nx, r,g,b);
		i++;
		rr+=resolution;
	}
}

void drawLineThin(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
									uint pix, uint piy,float angle, float length, ///< Start pixel, angle to draw and linelength
									unsigned char r,unsigned char g,unsigned char b){
	float dy = sin(angle);
	float dx = cos(angle);
	float rr=0;
	int i=0;
	uint x,y;
	while(rr<length){
		x = pix + i*dx;
		y = piy - i*dy;
		setRGBpix(RGB,x,y, Nx, r,g,b);
		i++;
		rr+=resolution;
	}
}

void drawCross(unsigned char *RGB, int Nx,int Ny,float resolution,   ///Grid parameters
							 uint pix, uint piy, float length, ///< Start pixel, angle to draw and linelength
							 unsigned char r,unsigned char g,unsigned char b){
	float angle = M_PI/4;
	for(int i=0;i<3;i++){
		drawLineThin(RGB, Nx,Ny,resolution, pix, piy,angle, length, r,g,b);
		angle+=M_PI/2;
	}
}


void saveJPEG(const char *fname, unsigned char *rgb,int Nx, int Ny){
		CJPEGEncoder encoder;
		encoder.SetRGBDataPtr((const char *)rgb,Nx,Ny);
		FILE *f = fopen(fname,"wb");
		fwrite(encoder.GetJPEGDataPtr(), 
					 encoder.GetJPEGDataSize(), 
					 1, f);
		fclose(f);
		
}
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////



int main(int argc, char *argv[]){
	UpdatePose matcher;
	
	FILE *f = fopen(matchfile, "rt");
	
	std::vector<Grid::pose> odoms;
	std::vector<Grid::pose> maps;
	std::vector<Grid::pose> vars;
	std::vector<int> inds;
	std::vector<int> indi;
	int cnt = 0;
	while(!feof(f)){
		Grid::pose odo,map,var;
		int ind;
		float w;
		fscanf(f,"%f %f %f %f %f %f %d %f\n",&odo.x,&odo.y,&odo.a,&map.x,&map.y,&map.a,&ind,&w);
		#warning "PERKELE poista joskus"
		
		odoms.push_back(odo);
		maps.push_back(map); ///<shoud be map
		inds.push_back(ind);
		var.x = w;
		var.y = w;
		var.a = (5.0*M_PI/180.0);
		vars.push_back(var);
		indi.push_back(cnt);
		cnt++;
	}
	fclose(f);
	
	matcher.initialize(odoms,4);
	matcher.update(maps, indi, vars);
	for(int i=0;i<10;i++) matcher.iterate();
	f = fopen(outfile,"wt");
	for(int i=0;i<maps.size();i++){			
		maps[i].set(matcher.Xpos[3*i],matcher.Xpos[3*i+1],matcher.Xpos[3*i+2]);
		
		fprintf(f,"%f %f %f %f %f %f %d\n",odoms[i].x,odoms[i].y,odoms[i].a,
						maps[i].x,maps[i].y,maps[i].a,inds[i]);
	}
	fclose(f);
	
	
	
	std::vector<Grid::pose> poses;
	std::vector<Grid::TScan *> scans;	
	Grid::pose current;
	
	
	FILE *log = fopen(poseset,"rt");
	float max_laser_dist = 50.0;
	///READ ALL THE MEASUREMENTS!
		while(!feof(log)){
				Grid::TScan *meas=new Grid::TScan();
				meas->alloc(600);
				int retv = readScanLine(log,*meas,current);
				if(retv == 0){
					meas->filt(max_laser_dist);
					scans.push_back(meas);
					poses.push_back(current);
				}else{delete meas;}
		}
		
		std::vector<Grid::pose> corr; //=poses;
		
		for(int i=0;i<maps.size();i++){
			int begin=inds[i];
			int end;
			if(i<maps.size()-1){ 
				end = inds[i+1]-1;
			}else end=poses.size()-1;
			
			fprintf(stderr,"From %d to %d\n",begin, end);
			corr.push_back(maps[i]);
			for(int j=begin;j<end;j++){
				Grid::pose diff;
				diff.setToDifferentialPose(poses[j+1] , poses[j]);
				Grid::pose npos = corr[j].integrateDifferential(diff);
				corr.push_back(npos);
			}
		}
		
		std::vector<Grid::pose> corr2; //=poses;
		
		
		corr2.push_back(maps[0]);
		
		for(int j=0;j<poses.size()-1;j++){
			Grid::pose diff;
			diff.setToDifferentialPose(poses[j+1] , poses[j]);
			Grid::pose npos = corr2[j].integrateDifferential(diff);
			corr2.push_back(npos);
		}

		TOccupancyGrid grid;
		TOccupancyGrid grid2(75.0, 75.0, 0.04, Grid::TCoord2D(0,0));
		grid2.setGridToValue(0.5);
		grid.load("rocla-map.occu");
		
		for(int i=0;i<corr.size();i++){
			grid.setScanFillProb(corr[i].x,corr[i].y,corr[i].a, *(scans[i]),0.3,0.9);
			grid2.setScanFillProb(corr[i].x,corr[i].y,corr[i].a, *(scans[i]),0.3,0.9);
		}
		int Nx,Ny;
		grid.getDim(Nx,Ny);
		uchar *rgb = (uchar *) malloc(3*Nx*Ny*sizeof(uchar));
		grid.fillRGB(rgb, 1.0/255.0);
		
		for(int i=0;i<corr.size();i++){
			Grid::TPixel pix = grid.toPixel(Grid::TCoord2D(corr[i].x,corr[i].y));
			if(pix.isValid(Nx,Ny)) setRGBpixBold(rgb,pix.x,pix.y, Nx, 0,255, 0);
		}
		
		for(int i=0;i<corr2.size();i++){
			Grid::TPixel pix = grid.toPixel(Grid::TCoord2D(corr2[i].x,corr2[i].y));
			if(pix.isValid(Nx,Ny)) setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,255, 0);
		}
		
		for(int i=0;i<maps.size();i++){
			Grid::TPixel pix = grid.toPixel(Grid::TCoord2D(maps[i].x,maps[i].y));
			if(pix.isValid(Nx,Ny)) setRGBpixBold(rgb,pix.x,pix.y, Nx, 255,0, 0);
		}
		
		grid2.save("Global-Map.occu");
		
		saveJPEG("matched_map.jpg", rgb,Nx, Ny);
	
	return 0;
}